; -*- mode: lisp; -*-

;
; soccerbot with composite bodies
;

(RSG 0 1)
( 
    ;
    ; define constants for the robot parts
    ;

    ; feet
    (def $FootLength 0.6)
    (def $FootWidth 0.956)
    (def $FootHeight 0.095)
    (def $FootMass 0.1)

    ; static box at the ankles
    (def $AnkleBoxLength 0.355)
    (def $AnkleBoxWidth 0.143)
    (def $AnkleBoxHeight 0.476)

    ; shanks
    (def $ShankLength 0.52)
    (def $ShankWidth 0.45)
    (def $ShankHeight 0.964)
    (def $ShankMass 0.25)

    ; thighs
    (def $ThighLength 0.56)
    (def $ThighWidth 0.45)
    (def $ThighHeight 1.3)
    (def $ThighMass 0.25)

    ; hips
    (def $HipLength 0.273)
    (def $HipWidth 0.273)
    (def $HipHeight 0.3)
    (def $HipMass 0.1)

    ; hand1
    (def $HandBox1Length 0.082)
    (def $HandBox1Width 0.272) 
    (def $HandBox1Height 0.57)

    ; hand2
    (def $HandBox2Length 0.242)
    (def $HandBox2Width 0.265)
    (def $HandBox2Height 0.164)

    ; hand3
    (def $HandBox3Length 0.074)
    (def $HandBox3Width 0.272)
    (def $HandBox3Height 0.2)

    ; lower arms
    (def $LowerarmLength 0.445)
    (def $LowerarmWidth 0.316)
    (def $LowerarmHeight 0.6)
    (def $LowerarmMass 0.2)

    ; elbows
    (def $ElbowDirection 1)
    (def $ElbowRadius 0.134)
    (def $ElbowLength 0.3)

    ; upper arms
    (def $UpperarmLength 0.445)
    (def $UpperarmWidth 0.398)
    (def $UpperarmHeight 0.506)
    (def $UpperarmMass 0.2)

    ; shoulders
    (def $ShoulderLength 0.445)
    (def $ShoulderWidth 1.017)
    (def $ShoulderHeight 0.536)
    (def $ShoulderMass 0.5) 

    ; head
    (def $HeadRadius 0.39)
    (def $HeadMass 0.3)

    (def $UpperTorsoLength 1.37)
    (def $UpperTorsoWidth  0.96)
    (def $UpperTorsoHeight 1.00)
    (def $UpperTorsoMass   1.2)

    (def $LowerTorsoLength 1.32)
    (def $LowerTorsoWidth  0.55)
    (def $LowerTorsoHeight 0.55)
    (def $LowerTorsoMass   0.60)

    (def $TorsoCylinderDirection 1)   ; oriented along x-axis
    (def $TorsoCylinderRadius 0.1)
    (def $TorsoCylinderLength 0.3)

    ; 
    ; define joint stops
    ;

    (def $Rle1Min -90)       
    (def $Rle1Max 60)
    (def $Rle2Min -45)
    (def $Rle2Max 120)
    (def $Rle3Min -75)
    (def $Rle3Max 45)
    (def $Rle4Min -160)
    (def $Rle4Max 10)
    (def $Rle5Min -90)
    (def $Rle5Max 90)
    (def $Rle6Min -45)
    (def $Rle6Max 45)

    (def $Lle1Min -60)
    (def $Lle1Max 90)
    (def $Lle2Min -45)
    (def $Lle2Max 120)
    (def $Lle3Min -45)
    (def $Lle3Max 75)
    (def $Lle4Min -160)
    (def $Lle4Max 10)
    (def $Lle5Min -90)
    (def $Lle5Max 90)
    (def $Lle6Min -45)
    (def $Lle6Max 45)

    (def $Rae1Min -90)
    (def $Rae1Max 180)
    (def $Rae2Min -180)
    (def $Rae2Max 10)
    (def $Rae3Min -135)
    (def $Rae3Max 135)
    (def $Rae4Min -10)
    (def $Rae4Max 130)

    (def $Lae1Min -90)
    (def $Lae1Max 180)
    (def $Lae2Min -10)
    (def $Lae2Max 180)
    (def $Lae3Min -135)
    (def $Lae3Max 135)
    (def $Lae4Min -10)
    (def $Lae4Max 130)

    ;
    ; assemble the robot
    ;

    (def $UpperTorsoPosX 5)
    (def $UpperTorsoPosY 5)
    (def $UpperTorsoPosZ 3.75)

    ; calculate offset for the left shoulder cylinder
    (def $offsetLeftShoulderCylX (eval -1 * (eval $UpperTorsoLength / 2.0)))
    (def $offsetLeftShoulderCylY 0.0)
    (def $offsetLeftShoulderCylZ (eval (eval $UpperTorsoHeight / 2.0) - (eval $ShoulderHeight / 2.0)))
 
    ; calculate offset for the right shoulder cylinder
    (def $offsetRightShoulderCylX (eval $UpperTorsoLength / 2.0))
    (def $offsetRightShoulderCylY 0.0)
    (def $offsetRightShoulderCylZ (eval (eval $UpperTorsoHeight / 2.0) - (eval $ShoulderHeight / 2.0)))

    (nd Space
	(setName spaceman)

    ; assemble the upper torso ... 
    (nd AgentAspect
	; basics
        (setName body)
        (setLocalPos $UpperTorsoPosX $UpperTorsoPosY $UpperTorsoPosZ)
	;(importScene rsg/boxspheres/box.rsg $UpperTorsoLength $UpperTorsoWidth $UpperTorsoHeight $UpperTorsoMass matRed)
	(importScene rsg/agent/soccerbottorsocomp.rsg $UpperTorsoLength $UpperTorsoWidth $UpperTorsoHeight $UpperTorsoMass
	                                             $LowerTorsoLength $LowerTorsoWidth $LowerTorsoHeight $LowerTorsoMass
	                                             $HeadRadius $HeadMass)

	; we want to initialize
	(nd SingleMatInitEffector)
	;add a time perceptor
	(nd TimePerceptor)
	; install a node holding agent state data
        (nd AgentState
	    (setName AgentState)
	    ; add a gamestateperceptor
	    (nd GameStatePerceptor)
	    ; add hear perceptor
	    (nd HearPerceptor)
	)
	; add beam effector
	(nd BeamEffector)
	; add gyro rate sensor
	(nd GyroRatePerceptor (setName torso) )
	; add vision sensor
	(nd VisionPerceptor
	     (setSenseMyPos false)
	     (setStaticSenseAxis false)
	     (addNoise false)
	     )
        ; add say effector
        (nd SayEffector)

	; attach left shoulder cylinder (virtual)
	(nd Transform
	    (setLocalPos $offsetLeftShoulderCylX $offsetLeftShoulderCylY $offsetLeftShoulderCylZ)
	    (setLocalRotation 0 90 0)
	    (nd Capsule
		(setName leftshoulderpin)
		(setMaterial matDarkGrey)
		(setParams $TorsoCylinderRadius $TorsoCylinderLength)
		)
	    )
	; attach right shoulder cylinder (virtual)
	(nd Transform
	    (setLocalPos $offsetRightShoulderCylX $offsetRightShoulderCylY $offsetRightShoulderCylZ)
	    (setLocalRotation 0 90 0)
	    (nd Capsule
		(setName rightshoulderpin)
		(setMaterial matDarkGrey)
		(setParams $TorsoCylinderRadius $TorsoCylinderLength)
		)
	    )
      
	) ; done upper torso ...

    (def $LowerTorsoPosX $UpperTorsoPosX)
    (def $LowerTorsoPosY $UpperTorsoPosY)
    (def $LowerTorsoPosZ (eval $UpperTorsoPosZ 
				  - (eval $UpperTorsoHeight / 2.0) 
				  - (eval $LowerTorsoHeight / 2.0) 
				  + 0.25))
    
    ;;; -------------------------------------
    ;;; ARMS
    ;;; -------------------------------------

 
    ; {{{ BEGIN --- Left Shoulder (Solid Part) --- ;

    ; (1) Define Position
    (def $LeftShoulderPosX (eval $UpperTorsoPosX - 
				 (eval $UpperTorsoLength / 2.0) - 
				 (eval $ShoulderLength / 2.0) - 
				 (eval $TorsoCylinderLength / 2.0)
				 )
	 )
    (def $LeftShoulderPosY $UpperTorsoPosY)
    (def $LeftShoulderPosZ (eval $UpperTorsoPosZ + 
				 (eval $UpperTorsoHeight / 2.0) -
				 (eval $ShoulderHeight / 2.0)
				 )
	 )

    ; (2) Attach Left Shoulder
    (nd Transform
	; basics
        (setName leftshoulder)
        (setLocalPos $LeftShoulderPosX $LeftShoulderPosY $LeftShoulderPosZ)
        (importScene rsg/boxspheres/box.rsg $ShoulderLength $ShoulderWidth $ShoulderHeight $ShoulderMass matDarkGrey)

        ; install universal joint to connect to the torso
        (nd UniversalJoint
            (attach ../boxBody ../../body/boxBody)
            (setAnchor 0.0 0.0 0.0)   
            (setAxis1 1.0 0.0 0.0) ; move around the x-axis
            (setAxis2 0.0 1.0 0.0) ; move around the y-axis
	    
	    ; enable the joint motors
            (setMaxMotorForce 0 100)
            (setMaxMotorForce 1 100)           

	    ; install a perceptor
            (nd UniversalJointPerceptor (setName laj1_2) )

            ; install an effector to control the joint motors
            (nd UniversalJointEffector (setName lae1_2) )

	    ; set joint stops
	    ;(setLowStopDeg 0 $Lae1Min)
	    ;(setHighStopDeg 0 $Lae1Max)
	    ;(setLowStopDeg 1 $Lae2Min)
	    ;(setHighStopDeg 1 $Lae2Max)
	    )
	)

    ; }}} END

    ; {{{ BEGIN --- Left Shoulder (Solid Part) --- ;

    (def $RightShoulderPosX (eval $UpperTorsoPosX +
				  (eval $UpperTorsoLength / 2.0) + 
				  (eval $ShoulderLength / 2.0) + 
				  (eval $TorsoCylinderLength / 2.0)
				  )
	 )
    (def $RightShoulderPosY $UpperTorsoPosY)
    (def $RightShoulderPosZ $LeftShoulderPosZ)
    
    ; (2) Attach Right Shoulder
    (nd Transform
	; basics
        (setName rightshoulder)
        (setLocalPos $RightShoulderPosX $RightShoulderPosY $RightShoulderPosZ)
        (importScene rsg/boxspheres/box.rsg $ShoulderLength $ShoulderWidth $ShoulderHeight $ShoulderMass matDarkGrey)

        ;(setLocalRotation 0 0 180)

        ; install universal joint to connect to the torso
	(nd UniversalJoint		
	    (attach ../boxBody ../../body/boxBody)
	    (setAnchor 0.0 0.0 0.0)  
	    (setAxis1 1.0 0.0 0.0) ; move around the x-axis
	    (setAxis2 0.0 1.0 0.0) ; move around the y-axis

	    ; enable the joint motors
	    (setMaxMotorForce 0 100)
	    (setMaxMotorForce 1 100)           

            ; install a perceptor
	    (nd UniversalJointPerceptor (setName raj1_2) )

            ; install an effector to control the joint motors
	    (nd UniversalJointEffector (setName rae1_2) )

	    ; set joint stops
	    ;(setLowStopDeg 0 $Rae1Min)
	    ;(setHighStopDeg 0 $Rae1Max)
	    ;(setLowStopDeg 1 $Rae2Min)
	    ;(setHighStopDeg 1 $Rae2Max)
            )
	)
    
    ; }}} END 


    ; {{{ BEGIN --- Left Upper Arm --- ;

    ; (1) Define Position
    (def $LeftUpperArmPosX $LeftShoulderPosX)
    (def $LeftUpperArmPosY (eval $LeftShoulderPosY))
    (def $LeftUpperArmPosZ (eval $LeftShoulderPosZ - (eval $ShoulderHeight / 2.0) - (eval $UpperarmHeight / 2.0)))
    
    ; (2) Attach Left Upper Arm
    (nd Transform
        (setName leftupperarm)
        (setLocalPos $LeftUpperArmPosX $LeftUpperArmPosY $LeftUpperArmPosZ)
        (importScene rsg/boxspheres/box.rsg $UpperarmLength $UpperarmWidth $UpperarmHeight $UpperarmMass matGrey)

        ; install hinge joint to connect to the shoulder
        (nd HingeJoint
            (attach ../boxBody ../../leftshoulder/boxBody)
            (setAnchor 0.0 0.0 (eval $UpperarmHeight / 2.0))   
            (setAxis 2) ; move around z-axis
            
            ; enable the joint motors
            (setMaxMotorForce 0 100)

            ; install a perceptor
            (nd HingePerceptor (setName laj3) )

            ; install an effector to control the joint motors
            (nd HingeEffector (setName lae3) )
	    
	    ; set joint stops
	    ;(setLowStopDeg 0 $Lae3Min)
	    ;(setHighStopDeg 0 $Lae3Max)
	    )

	; static cylinder for the elbow
	(nd Transform
	    (setLocalPos 0 0 (eval -1 * (eval (eval $UpperarmHeight / 2.0) + (eval $ElbowRadius / 2.0))))
	    (setLocalRotation 0 90 0)
	    (nd Capsule	
		(setMaterial matGrey)
		(setParams $ElbowRadius $ElbowLength)
		)
	    )
	)

    ; }}} END

    ; {{{ BEGIN --- Right Upper Arm --- ;

    ; (1) Define Position
    (def $RightUpperArmPosX $RightShoulderPosX)
    (def $RightUpperArmPosY (eval $RightShoulderPosY))
    (def $RightUpperArmPosZ (eval $RightShoulderPosZ - (eval $ShoulderHeight / 2.0) - (eval $UpperarmHeight / 2.0)))

    ; (2) Attach Right Upper Arm
    (nd Transform
        (setName rightupperarm)
        (setLocalPos $RightUpperArmPosX $RightUpperArmPosY $RightUpperArmPosZ)
        (importScene rsg/boxspheres/box.rsg $UpperarmLength $UpperarmWidth $UpperarmHeight $UpperarmMass matGrey)

        ; install hinge joint to connect to the shoulder
        (nd HingeJoint
            (attach ../boxBody ../../rightshoulder/boxBody)
            (setAnchor 0 0 (eval $UpperarmHeight / 2.0))   
            (setAxis 2) ; move around z-axis
            
            ; enable the joint motors
            (setMaxMotorForce 0 100)

            ; install a perceptor
            (nd HingePerceptor (setName raj3) )

            ; install an effector to control the joint motors
            (nd HingeEffector (setName rae3) )

	    ; set joint stops
	    ;(setLowStopDeg 0 $Rae3Min)
	    ;(setHighStopDeg 0 $Rae3Max)
        )

	; static cylinder for the elbow
	(nd Transform
	    (setLocalPos 0 0 (eval -1 * (eval (eval $UpperarmHeight / 2.0) + (eval $ElbowRadius / 2.0))))
	    (setLocalRotation 0 90 0)
	    (nd Capsule
		(setMaterial matGrey)
		(setParams $ElbowRadius $ElbowLength)
		)
	    )
	)

    ; }}} END


    ; {{{ BEGIN --- Left Lower Arm --- ;

    ; (1) Define Position 
    (def $LeftLowerArmPosX $LeftUpperArmPosX)
    (def $LeftLowerArmPosY $LeftUpperArmPosY)
    (def $LeftLowerArmPosZ (eval $LeftUpperArmPosZ -
				    (eval $UpperarmHeight / 2.0) -
				    $ElbowRadius -
				    (eval $LowerarmHeight / 2.0)
			       )
     )

    ; (2) Attach Left Lower Arm
    (nd Transform

	; basic configuration
	(setName leftlowerarm)
        (setLocalPos $LeftLowerArmPosX $LeftLowerArmPosY $LeftLowerArmPosZ)
        
        (importScene rsg/agent/soccerbotleftlowerarmcomp.rsg matDarkGrey matGrey)
    
        (nd HingeJoint
            (attach ../boxBody ../../leftupperarm/boxBody)
            (setAnchor 0 0 (eval $LowerarmHeight / 2.0))   
            (setAxis 0) ; move around x-axis
            
            ; enable the joint motors
            (setMaxMotorForce 0 100)

            ; install a perceptor
            (nd HingePerceptor (setName laj4))

            ; install an effector to control the joint motors
            (nd HingeEffector (setName lae4))

	    ; set joint stops
	    ;(setLowStopDeg 0 $Lae4Min)
	    ;(setHighStopDeg 0 $Lae4Max)
        )
    )
    ; }}} END 

    ; {{{ BEGIN --- Right Lower Arm --- ;

    ; (1) Define Position
    (def $RightLowerArmPosX $RightUpperArmPosX)
    (def $RightLowerArmPosY $RightUpperArmPosY)
    (def $RightLowerArmPosZ (eval $RightUpperArmPosZ -
				    (eval $UpperarmHeight / 2.0) -
				    $ElbowRadius -
				    (eval $LowerarmHeight / 2.0)
			       )
     )

    ; (2) Attach Right Lower Arm
    (nd Transform
        (setName rightlowerarm)
        (setLocalPos $RightLowerArmPosX $RightLowerArmPosY $RightLowerArmPosZ)

        (importScene rsg/agent/soccerbotrightlowerarmcomp.rsg matDarkGrey matGrey)

        ; install hinge joint to connect to the upper arm
        (nd HingeJoint
            (attach ../boxBody ../../rightupperarm/boxBody)
            (setAnchor 0 0 (eval $LowerarmHeight / 2.0))   
            (setAxis 0) ; move around x-axis
            
            ; enable the joint motors
            (setMaxMotorForce 0 100)

            ; install a perceptor
            (nd HingePerceptor (setName raj4))

            ; install an effector to control the joint motors
            (nd HingeEffector (setName rae4))

	    ; set joint stops
	    ;(setLowStopDeg 0 $Rae4Min)
	    ;(setHighStopDeg 0 $Rae4Max)
        )

     )

     ; }}} END




    ;;; -------------------------------------
    ;;; LEGS
    ;;; -------------------------------------
    
    (def $LeftHipPosX (eval $LowerTorsoPosX - (eval $LowerTorsoLength / 4.0) - 0.06))
    (def $LeftHipPosY $LowerTorsoPosY)
    (def $LeftHipPosZ (eval $LowerTorsoPosZ - (eval $LowerTorsoHeight / 2.0 ) - (eval $HipHeight / 2.0)))

    (def $RightHipPosX (eval $LowerTorsoPosX + (eval $LowerTorsoLength / 4.0) + 0.06))
    (def $RightHipPosY $LowerTorsoPosY)
    (def $RightHipPosZ (eval $LowerTorsoPosZ - (eval $LowerTorsoHeight / 2.0) - (eval $HipHeight / 2.0 )))

    ; left hip
    (nd Transform
        (setName lefthip)
        (setLocalPos $LeftHipPosX $LeftHipPosY $LeftHipPosZ)
        (importScene rsg/boxspheres/box.rsg $HipLength $HipWidth $HipHeight $HipMass matDarkGrey)

        ; install hinge joint to connect to the torso
        (nd HingeJoint
            (attach ../boxBody ../../body/boxBody)
            (setAnchor 0 0 (eval $HipHeight * 0.5))   
            (setAxis 2) ; move around z-axis
            
            ; enable the joint motors
            (setMaxMotorForce 0 100)

            ; install a perceptor
            (nd HingePerceptor (setName llj1))

            ; install an effector to control the joint motors
            (nd HingeEffector (setName lle1))

	    ; set joint stops
	    ;(setLowStopDeg 0 $Lle1Min)
	    ;(setHighStopDeg 0 $Lle1Max)
        )
    )

    ; right hip
    (nd Transform
        (setName righthip)
        (setLocalPos $RightHipPosX $RightHipPosY $RightHipPosZ)
        (importScene rsg/boxspheres/box.rsg $HipLength $HipWidth $HipHeight $HipMass matDarkGrey)

        ; install hinge joint to connect to the torso
        (nd HingeJoint
            (attach ../boxBody ../../body/boxBody)
            (setAnchor 0 0 (eval $HipHeight * 0.5))   
            (setAxis 2) ; move around z-axis
            
            ; enable the joint motors
            (setMaxMotorForce 0 100)

            ; install a perceptor
            (nd HingePerceptor (setName rlj1))

            ; install an effector to control the joint motors
            (nd HingeEffector (setName rle1))

	    ; set joint stops
	    ;(setLowStopDeg 0 $Rle1Min)
	    ;(setHighStopDeg 0 $Rle1Max)
        )
    )

    (def $LeftThighPosX $LeftHipPosX)
    (def $LeftThighPosY $LeftHipPosY)
    (def $LeftThighPosZ (eval $LeftHipPosZ - (eval $HipHeight / 2.0) - (eval $ThighHeight / 2.0) ))

    (def $RightThighPosX $RightHipPosX)
    (def $RightThighPosY $RightHipPosY)
    (def $RightThighPosZ (eval $RightHipPosZ - (eval $HipHeight / 2.0) - (eval $ThighHeight / 2.0) ))

    ; left thigh
    (nd Transform
        (setName leftthigh)
        (setLocalPos $LeftThighPosX $LeftThighPosY $LeftThighPosZ)
        (importScene rsg/boxspheres/box.rsg $ThighLength $ThighWidth $ThighHeight $ThighMass matDarkGrey)

        ; install hinge joint to connect to the torso
        (nd UniversalJoint
            (attach ../boxBody ../../lefthip/boxBody)
            (setAnchor 0.0 0.0 (eval $ThighHeight / 2.0))   
            (setAxis1 1.0 0.0 0.0) ; move around the x-axis
            (setAxis2 0.0 1.0 0.0) ; move around the y-axis

            ; enable the joint motors
            (setMaxMotorForce 0 100)
            (setMaxMotorForce 1 100)           

            ; install a perceptor
            (nd UniversalJointPerceptor (setName llj2_3))

            ; install an effector to control the joint motors
            (nd UniversalJointEffector (setName lle2_3))

	    ; set joint stops
	    ;(setLowStopDeg 0 $Lle2Min)
	    ;(setHighStopDeg 0 $Lle2Max)
	    ;(setLowStopDeg 1 $Lle3Min)
	    ;(setHighStopDeg 1 $Lle3Max)
        )

        ; attach left knee cylinder (virtual)
	(nd Transform
	      (setLocalPos 0 0 (eval (eval $ThighHeight * -0.5) - 0.025))
	      (setLocalRotation 0 90 0)
	      (nd Capsule
		    (setMaterial matRed)
		    (setParams 0.1 (eval $ThighLength - 0.2))
		    )
	      )
    )


    ; right thigh
    (nd Transform
        (setName rightthigh)
        (setLocalPos $RightThighPosX $RightThighPosY $RightThighPosZ)
        (importScene rsg/boxspheres/box.rsg $ThighLength $ThighWidth $ThighHeight $ThighMass matDarkGrey)

        ; install hinge joint to connect to the torso
        (nd UniversalJoint
            (attach ../boxBody ../../righthip/boxBody)
            (setAnchor 0.0 0.0 (eval $ThighHeight / 2.0))   
            (setAxis1 1.0 0.0 0.0) ; move around the x-axis
            (setAxis2 0.0 1.0 0.0) ; move around the y-axis

            ; enable the joint motors
            (setMaxMotorForce 0 100)
            (setMaxMotorForce 1 100)           

            ; install a perceptor
            (nd UniversalJointPerceptor (setName rlj2_3))

            ; install an effector to control the joint motors
            (nd UniversalJointEffector (setName rle2_3))

	    ; set joint stops
	    ;(setLowStopDeg 0 $Rle2Min)
	    ;(setHighStopDeg 0 $Rle2Max)
	    ;(setLowStopDeg 1 $Rle3Min)
	    ;(setHighStopDeg 1 $Rle3Max)

        ; attach left knee cylinder (virtual)
	(nd Transform
	      (setLocalPos 0 0 (eval (eval $ThighHeight * -0.5) - 0.025))
	      (setLocalRotation 0 90 0)
	      (nd Capsule
		    (setMaterial matRed)
		    (setParams 0.1 (eval $ThighLength - 0.2))
		    )
	      )
        )
    )

    (def $LeftShankPosX $LeftThighPosX)
    (def $LeftShankPosY $LeftThighPosY)
    (def $LeftShankPosZ (eval $LeftThighPosZ - (eval $ThighHeight / 2.0) - (eval $ShankHeight / 2.0) - 0.05))

    (def $RightShankPosX $RightThighPosX)
    (def $RightShankPosY $RightThighPosY)
    (def $RightShankPosZ (eval $RightThighPosZ - (eval $ThighHeight / 2.0) - (eval $ShankHeight / 2.0) - 0.05))

    ; left shank
    (nd Transform
        (setName leftshank)
        (setLocalPos $LeftShankPosX $LeftShankPosY $LeftShankPosZ)
        (importScene rsg/boxspheres/box.rsg $ShankLength $ShankWidth $ShankHeight $ShankMass matDarkGrey)

        ; install hinge joint to connect to the thigh
        (nd HingeJoint
            (attach ../boxBody ../../leftthigh/boxBody)
            (setAnchor 0.0 0.0 (eval $ShankHeight / 2.0))   
            (setAxis 0) ; move around x-axis
            
            ; enable the joint motors
            (setMaxMotorForce 0 100)

            ; install a perceptor
            (nd HingePerceptor (setName llj4)
            )

            ; install an effector to control the joint motors
            (nd HingeEffector (setName lle4))

	    ; set joint stops
	    ;(setLowStopDeg 0 $Lle4Min)
	    ;(setHighStopDeg 0 $Lle4Max)
        )
    )

    ; right shank
    (nd Transform
        (setName rightshank)
        (setLocalPos $RightShankPosX $RightShankPosY $RightShankPosZ)
        (importScene rsg/boxspheres/box.rsg $ShankLength $ShankWidth $ShankHeight $ShankMass matDarkGrey)

        ; install hinge joint to connect to the thigh
        (nd HingeJoint
            (attach ../boxBody ../../rightthigh/boxBody)
            (setAnchor 0.0 0.0 (eval $ShankHeight / 2.0))
            (setAxis 0) ; move around x-axis
            
            ; enable the joint motors
            (setMaxMotorForce 0 100)

            ; install a perceptor
            (nd HingePerceptor (setName rlj4))

            ; install an effector to control the joint motors
            (nd HingeEffector (setName rle4))

	    ; set joint stops
	    ;(setLowStopDeg 0 $Rle4Min)
	    ;(setHighStopDeg 0 $Rle4Max)
        )
    )

    ; left foot
    (nd Transform
        (setName leftfoot)
        (setLocalPos $LeftShankPosX 
		     $LeftShankPosY
		     (eval $LeftShankPosZ - (eval $ShankHeight / 2.0) - (eval $FootHeight / 2.0) - 0.05)
	 )

	; install touch perceptor
	;(nd TouchPerceptor (setName lf))

	; install FRP
	(nd ForceResistancePerceptor
		(setName lf)
	)

	(importScene rsg/boxspheres/box_with_handler.rsg $FootLength $FootWidth $FootHeight $FootMass matDarkGrey)

        ; install hinge joint to connect to the shank
        (nd UniversalJoint
           (attach ../boxBody ../../leftshank/boxBody)
           (setAnchor 0.0 0.0 (eval $FootHeight * 3.0))   
           (setAxis1 1.0 0.0 0.0) ; move around the x-axis
           (setAxis2 0.0 1.0 0.0) ; move around the y-axis

           ; enable the joint motors
           (setMaxMotorForce 0 100)
           (setMaxMotorForce 1 100)           

            ; install a perceptor
            (nd UniversalJointPerceptor (setName llj5_6))

            ; install an effector to control the joint motors
            (nd UniversalJointEffector (setName lle5_6))

	    ; set joint stops
	    ;(setLowStopDeg 0 $Lle5Min)
	    ;(setHighStopDeg 0 $Lle5Max)
	    ;(setLowStopDeg 1 $Lle6Min)
	    ;(setHighStopDeg 1 $Lle6Max)
        )

	; static box at the back of the foot
	(nd Transform
	     (setLocalPos 0.0
			  (eval -1 * (eval (eval $FootWidth / 2.0) - (eval $AnkleBoxWidth / 2.0)))
			  (eval (eval $FootHeight / 2.0) + (eval $AnkleBoxHeight / 2.0))
	      )
	     (nd Box
		   (setMaterial matGrey)
		   (setExtents $AnkleBoxLength $AnkleBoxWidth $AnkleBoxHeight)
	     )
	)
    )

    ; right foot
    (nd Transform
        (setName rightfoot)
        (setLocalPos $RightShankPosX 
		     $RightShankPosY
		     (eval $RightShankPosZ - (eval $ShankHeight / 2.0) - (eval $FootHeight / 2.0) - 0.05)
	 )

        ; install touch perceptor
	;(nd TouchPerceptor (setName rf))

	; install FRP
	(nd ForceResistancePerceptor
		(setName rf)
	)

        (importScene rsg/boxspheres/box_with_handler.rsg $FootLength $FootWidth $FootHeight $FootMass matDarkGrey)
						
        ; install hinge joint to connect to the shank
        (nd UniversalJoint
            (attach ../boxBody ../../rightshank/boxBody)
            (setAnchor 0.0 0.0 (eval $FootHeight * 3.0))   
            (setAxis1 1.0 0.0 0.0) ; move around the x-axis
            (setAxis2 0.0 1.0 0.0) ; move around the y-axis

            ; enable the joint motors
            (setMaxMotorForce 0 100)
            (setMaxMotorForce 1 100)           

            ; install a perceptor
            (nd UniversalJointPerceptor (setName rlj5_6))

            ; install an effector to control the joint motors
            (nd UniversalJointEffector (setName rle5_6))

	    ; set joint stops
	    ;(setLowStopDeg 0 $Rle5Min)
	    ;(setHighStopDeg 0 $Rle5Max)
	    ;(setLowStopDeg 1 $Rle6Min)
	    ;(setHighStopDeg 1 $Rle6Max)
        )

	; static box at the back of the foot
	(nd Transform
	     (setLocalPos 0.0
			  (eval -1 * (eval (eval $FootWidth / 2.0) - (eval $AnkleBoxWidth / 2.0)))
			  (eval (eval $FootHeight / 2.0) + (eval $AnkleBoxHeight / 2.0))
	      )
	     (nd Box
		   (setMaterial matGrey)
		   (setExtents $AnkleBoxLength $AnkleBoxWidth $AnkleBoxHeight)
	     )
	)
    )

) ; of space
) ; EOF

